#ifndef __MATH_PLANE_H__
#define __MATH_PLANE_H__

/*
===============================================================================

3D plane with equation: a * x + b * y + c * z + d = 0

===============================================================================
*/


namespace inf {
    class Vec3;
    class Mat3;

#define	ON_EPSILON					0.1f
#define DEGENERATE_DIST_EPSILON		1e-4f

#define	SIDE_FRONT					0
#define	SIDE_BACK					1
#define	SIDE_ON						2
#define	SIDE_CROSS					3

    // plane sides
#define PLANESIDE_FRONT				0
#define PLANESIDE_BACK				1
#define PLANESIDE_ON				2
#define PLANESIDE_CROSS				3

    // plane types
#define PLANETYPE_X					0
#define PLANETYPE_Y					1
#define PLANETYPE_Z					2
#define PLANETYPE_NEGX				3
#define PLANETYPE_NEGY				4
#define PLANETYPE_NEGZ				5
#define PLANETYPE_TRUEAXIAL			6	// all types < 6 are true axial planes
#define PLANETYPE_ZEROX				6
#define PLANETYPE_ZEROY				7
#define PLANETYPE_ZEROZ				8
#define PLANETYPE_NONAXIAL			9

    class Plane {
    public:
        Plane( void );
        Plane( float a, float b, float c, float d );
        Plane( const Vec3 &normal, const float dist );

        float			operator[]( int index ) const;
        float &			operator[]( int index );
        Plane			operator-() const;						// flips plane
        Plane &		    operator=( const Vec3 &v );			// sets normal and sets Plane::d to zero
        Plane			operator+( const Plane &p ) const;	// add plane equations
        Plane			operator-( const Plane &p ) const;	// subtract plane equations
        Plane &		    operator*=( const Mat3 &m );			// normal() *= m

        bool			compare( const Plane &p ) const;						// exact compare, no epsilon
        bool			compare( const Plane &p, const float epsilon ) const;	// compare with epsilon
        bool			compare( const Plane &p, const float normalEps, const float distEps ) const;	// compare with epsilon
        bool			operator==(	const Plane &p ) const;					// exact compare, no epsilon
        bool			operator!=(	const Plane &p ) const;					// exact compare, no epsilon

        void			zero( void );							// zero plane
        void			SetNormal( const Vec3 &normal );		// sets the normal
        const Vec3 &	normal( void ) const;					// reference to const normal
        Vec3 &		    normal( void );							// reference to normal
        float			normalize( bool fixDegenerate = true );	// only normalizes the plane normal, does not adjust d
        bool			fix_degenerate_normal( void );			// fix degenerate normal
        bool			fix_degeneracies( float distEpsilon );	// fix degenerate normal and dist
        float			dist( void ) const;						// returns: -d
        void			set_dist( const float dist );			// sets: d = -dist
        int				type( void ) const;						// returns plane type
        void			set( float x, float y, float z, float dist );

        bool			from_points( const Vec3 &p1, const Vec3 &p2, const Vec3 &p3, bool fixDegenerate = true );

        bool			from_points_high_precision( const Vec3 &p0, const Vec3 &p1, const Vec3 &p2, bool fixDegenerate );

        bool			from_Vecs( const Vec3 &dir1, const Vec3 &dir2, const Vec3 &p, bool fixDegenerate = true );
        void			fit_through_point( const Vec3 &p );	// assumes normal is valid
        bool			height_fit( const Vec3 *points, const int numPoints );
        Plane			translate( const Vec3 &translation ) const;
        Plane &		    translate_self( const Vec3 &translation );
        Plane			rotate( const Vec3 &origin, const Mat3 &axis ) const;
        Plane &		    rotate_self( const Vec3 &origin, const Mat3 &axis );

        float			distance( const Vec3 &v ) const;
        int				side( const Vec3 &v, const float epsilon = 0.0f ) const;

        bool			line_intersection( const Vec3 &start, const Vec3 &end ) const;
        bool			line_intersection( const Vec3 &start, const Vec3 &end, float &fraction ) const;
        // intersection point is start + dir * scale
        bool			ray_intersection( const Vec3 &start, const Vec3 &dir, float &scale ) const;
        bool			plane_intersection( const Plane &plane, Vec3 &start, Vec3 &dir ) const;

        int				get_dimension( void ) const;

        const Vec4 &	to_Vec4( void ) const;
        Vec4 &		    to_Vec4( void );
        const float *	to_float_ptr( void ) const;
        float *			to_float_ptr( void );
        const char *	to_string( int precision = 2 ) const;

        static const Plane ORIGIN;
    private:
        float			a;
        float			b;
        float			c;
        float			d;
    };


    INF_INLINE Plane::Plane( void ) {
    }

    INF_INLINE Plane::Plane( float a, float b, float c, float d ) {
        this->a = a;
        this->b = b;
        this->c = c;
        this->d = d;
    }

    INF_INLINE Plane::Plane( const Vec3 &normal, const float dist ) {
        this->a = normal.x;
        this->b = normal.y;
        this->c = normal.z;
        this->d = -dist;
    }

    INF_INLINE float Plane::operator[]( int index ) const {
        return ( &a )[ index ];
    }

    INF_INLINE float& Plane::operator[]( int index ) {
        return ( &a )[ index ];
    }

    INF_INLINE Plane Plane::operator-( void ) const {
        return Plane( -a, -b, -c, -d );
    }

    INF_INLINE Plane &Plane::operator=( const Vec3 &v ) { 
        a = v.x;
        b = v.y;
        c = v.z;
        d = 0;
        return *this;
    }

    INF_INLINE Plane Plane::operator+( const Plane &p ) const {
        return Plane( a + p.a, b + p.b, c + p.c, d + p.d );
    }

    INF_INLINE Plane Plane::operator-( const Plane &p ) const {
        return Plane( a - p.a, b - p.b, c - p.c, d - p.d );
    }

    INF_INLINE Plane &Plane::operator*=( const Mat3 &m ) {
        normal() *= m;
        return *this;
    }

    INF_INLINE bool Plane::compare( const Plane &p ) const {
        return ( a == p.a && b == p.b && c == p.c && d == p.d );
    }

    INF_INLINE bool Plane::compare( const Plane &p, const float epsilon ) const {
        if ( math::fabs( a - p.a ) > epsilon ) {
            return false;
        }

        if ( math::fabs( b - p.b ) > epsilon ) {
            return false;
        }

        if ( math::fabs( c - p.c ) > epsilon ) {
            return false;
        }

        if ( math::fabs( d - p.d ) > epsilon ) {
            return false;
        }

        return true;
    }

    INF_INLINE bool Plane::compare( const Plane &p, const float normalEps, const float distEps ) const {
        if ( math::fabs( d - p.d ) > distEps ) {
            return false;
        }
        if ( !normal().compare( p.normal(), normalEps ) ) {
            return false;
        }
        return true;
    }

    INF_INLINE bool Plane::operator==( const Plane &p ) const {
        return compare( p );
    }

    INF_INLINE bool Plane::operator!=( const Plane &p ) const {
        return !compare( p );
    }

    INF_INLINE void Plane::zero( void ) {
        a = b = c = d = 0.0f;
    }

    INF_INLINE void Plane::SetNormal( const Vec3 &normal ) {
        a = normal.x;
        b = normal.y;
        c = normal.z;
    }

    INF_INLINE const Vec3 &Plane::normal( void ) const {
        return *reinterpret_cast<const Vec3 *>(&a);
    }

    INF_INLINE Vec3 &Plane::normal( void ) {
        return *reinterpret_cast<Vec3 *>(&a);
    }

    INF_INLINE float Plane::normalize( bool fixDegenerate ) {
        float length = reinterpret_cast<Vec3 *>(&a)->normalize();

        if ( fixDegenerate ) {
            fix_degenerate_normal();
        }
        return length;
    }

    INF_INLINE bool Plane::fix_degenerate_normal( void ) {
        return normal().fix_degenerate_normal();
    }

    INF_INLINE bool Plane::fix_degeneracies( float distEpsilon ) {
        bool fixedNormal = fix_degenerate_normal();
        // only fix dist if the normal was degenerate
        if ( fixedNormal ) {
            if ( math::fabs( d - math::rint( d ) ) < distEpsilon ) {
                d = math::rint( d );
            }
        }
        return fixedNormal;
    }

    INF_INLINE float Plane::dist( void ) const {
        return -d;
    }

    INF_INLINE void Plane::set_dist( const float dist ) {
        d = -dist;
    }

    INF_INLINE void Plane::set( float a, float b, float c, float d ) {
        this->a = a;
        this->b = b;
        this->c = c;
        this->d = d;
    }

    INF_INLINE bool Plane::from_points( const Vec3 &p1, const Vec3 &p2, const Vec3 &p3, bool fixDegenerate ) {
        normal() = (p1 - p2).cross( p3 - p2 );
        if ( normalize( fixDegenerate ) == 0.0f ) {
            return false;
        }
        d = -( normal() * p2 );
        return true;
    }


    INF_INLINE bool Plane::from_points_high_precision( const Vec3 &p0, const Vec3 &p1, const Vec3 &p2, bool fixDegenerate ) {
        // Take the cross product of the edge directions of the two shortest edges for maximum precision.
        // The shortest two edges of a triangle are also the two edges that are most orthogonal to each other.
#if 0
        float l0 = ( p2 - p1 ).length_sqr();
        float l1 = ( p0 - p2 ).length_sqr();
        float l2 = ( p1 - p0 ).length_sqr();

        if ( l0 > l1 && l0 > l2 ) {
            Vec3 v1 = p1 - p0;
            Vec3 v2 = p2 - p0;
            normal() = v1.cross( v2 );
        } else if ( l1 > l0 && l1 > l2 ) {
            Vec3 v1 = p2 - p1;
            Vec3 v2 = p0 - p1;
            normal() = v1.cross( v2 );
        } else {
            Vec3 v1 = p0 - p2;
            Vec3 v2 = p1 - p2;
            normal() = v1.cross( v2 );
        }
        bool r = normalize( fixDegenerate ) != 0.0f;
        fit_through_point( p0 );
        return r;
#else
        const Vec3 *p[3] = { &p0, &p1, &p2 };
        float l0 = ( p2 - p1 ).length_sqr();
        float l1 = ( p0 - p2 ).length_sqr();
        float l2 = ( p1 - p0 ).length_sqr();
        int index = math::max3_index( l0, l1, l2 );
        Vec3 v1 = *p[(index+1)%3] - *p[index];
        Vec3 v2 = *p[(index+2)%3] - *p[index];
        normal() = v1.cross( v2 );
        bool r = normalize( fixDegenerate ) != 0.0f;
        fit_through_point( p0 );
        return r;
#endif
    }

    INF_INLINE bool Plane::from_Vecs( const Vec3 &dir1, const Vec3 &dir2, const Vec3 &p, bool fixDegenerate ) {
        normal() = dir1.cross( dir2 );
        if ( normalize( fixDegenerate ) == 0.0f ) {
            return false;
        }
        d = -( normal() * p );
        return true;
    }

    INF_INLINE void Plane::fit_through_point( const Vec3 &p ) {
        d = -( normal() * p );
    }

    INF_INLINE Plane Plane::translate( const Vec3 &translation ) const {
        return Plane( a, b, c, d - translation * normal() );
    }

    INF_INLINE Plane &Plane::translate_self( const Vec3 &translation ) {
        d -= translation * normal();
        return *this;
    }

    INF_INLINE Plane Plane::rotate( const Vec3 &origin, const Mat3 &axis ) const {
        Plane p;
        p.normal() = normal() * axis;
        p.d = d + origin * normal() - origin * p.normal();
        return p;
    }

    INF_INLINE Plane &Plane::rotate_self( const Vec3 &origin, const Mat3 &axis ) {
        d += origin * normal();
        normal() *= axis;
        d -= origin * normal();
        return *this;
    }

    INF_INLINE float Plane::distance( const Vec3 &v ) const {
        return a * v.x + b * v.y + c * v.z + d;
    }

    INF_INLINE int Plane::side( const Vec3 &v, const float epsilon ) const {
        float dist = distance( v );
        if ( dist > epsilon ) {
            return PLANESIDE_FRONT;
        }
        else if ( dist < -epsilon ) {
            return PLANESIDE_BACK;
        }
        else {
            return PLANESIDE_ON;
        }
    }

    INF_INLINE bool Plane::line_intersection( const Vec3 &start, const Vec3 &end ) const {
        float d1, d2, fraction;

        d1 = normal() * start + d;
        d2 = normal() * end + d;
        if ( d1 == d2 ) {
            return false;
        }
        if ( d1 > 0.0f && d2 > 0.0f ) {
            return false;
        }
        if ( d1 < 0.0f && d2 < 0.0f ) {
            return false;
        }
        fraction = ( d1 / ( d1 - d2 ) );
        return ( fraction >= 0.0f && fraction <= 1.0f );
    }

    INF_INLINE bool Plane::line_intersection( const Vec3 &start, const Vec3 &end, float &fraction ) const {
        float d1, d2;

        d1 = normal() * start + d;
        d2 = normal() * end + d;
        if ( d1 == d2 ) {
            return false;
        }
        if ( d1 > 0.0f && d2 > 0.0f ) {
            return false;
        }
        if ( d1 < 0.0f && d2 < 0.0f ) {
            return false;
        }
        fraction = ( d1 / ( d1 - d2 ) );
        return ( fraction >= 0.0f && fraction <= 1.0f );
    }

    INF_INLINE bool Plane::ray_intersection( const Vec3 &start, const Vec3 &dir, float &scale ) const {
        float d1, d2;

        d1 = normal() * start + d;
        d2 = normal() * dir;
        if ( d2 == 0.0f ) {
            return false;
        }
        scale = -( d1 / d2 );
        return true;
    }

    INF_INLINE int Plane::get_dimension( void ) const {
        return 4;
    }

    INF_INLINE const Vec4 &Plane::to_Vec4( void ) const {
        return *reinterpret_cast<const Vec4 *>(&a);
    }

    INF_INLINE Vec4 &Plane::to_Vec4( void ) {
        return *reinterpret_cast<Vec4 *>(&a);
    }

    INF_INLINE const float *Plane::to_float_ptr( void ) const {
        return reinterpret_cast<const float *>(&a);
    }

    INF_INLINE float *Plane::to_float_ptr( void ) {
        return reinterpret_cast<float *>(&a);
    }


}   // ::inf

#endif /* !__MATH_PLANE_H__ */
